//节点3
//功能介绍：
//-----------1、地面站模拟（信息模拟&发送模拟）
// ----------2、通过回调函数，获取无人机当前的GPS位置信息
// ----------3、按照通信协议构造信息
// ----------4、从串口接收信息，并进行解析&分类存储
// ----------5、打印or发布信息给上层节点
#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口包 
#include <std_msgs/String.h> 
#include <std_msgs/Empty.h> 
#include <std_msgs/Bool.h>
#include <std_msgs/UInt8.h>
#include "serialport/ALL_UAVs_LLA.h"
#include "sensor_msgs/NavSatFix.h"
#include <string>
#include <time.h>
#include "serialport/data_make.h" 
#include <wrzf_pkg/DroneState.h>

using namespace std;
serial::Serial ser; //声明串口对象 
uint8_t split(double data, int position);    //将64位的double变成uint32_t再变成uint8_t
double merge(uint8_t first, uint8_t second, uint8_t third, uint8_t fourth); //将4个uint8_t转成uint32_t,再变成double
uint8_t data_crc(uint8_t *p_data, int len_data);   //接收校验
const int Blen_child = 17;    //子节点的数据长度
const int Blen_host  = 54;    //主节点的数据长度
const int Blen_stop  = 9;     //子节点信息发送许可证---信息
const int Blen_life  = 6;     //子节点信息发送许可证---信息
uint8_t recv_buff[140];
uint8_t send_buff[17];
uint8_t life_buff[6];
int len_send, lend_recv;
int uav_ID;
std_msgs::Bool takeoff_flag;  
uint8_t takeoff_flag_recv;

struct uav_MSG
{
    double longitude;
    double latitude;
    double altitude;
};
uav_MSG uav={},uav4={},          uav1={},          uav2={},          uav3={};
uav_MSG               uav4_last={},uav1_last={},uav2_last={},uav3_last={};

// 【回调函数】无人机当前位置
wrzf_pkg::DroneState uav_current_state;
void uav_lla_cb(const wrzf_pkg::DroneState::ConstPtr& msg)
{
    uav_current_state = *msg;
}
/**************************************************************************************************/
const int frequence  = 40;    
const int ttimeout  = 7;    
const int readnum  =120;     
int numtime=0;
/**************************************************************************************************/
int main (int argc, char** argv) 
{ 
    ros::init(argc, argv, "child3_node");   //初始化节点 
    ros::NodeHandle nh;                                 //声明节点句柄
    ros::Rate loop_rate(frequence);                            //设置循环频率，括号内参数即为频率 
    cout << "start child3_node" << endl;
    
    //【订阅】无人机的自身位置
    ros::Subscriber uav_lla_sub = nh.subscribe<wrzf_pkg::DroneState>("/px4_command/drone_state", 10,uav_lla_cb);
    //【发布】所有无人机的位置信息
    ros::Publisher uavs_lla_pub = nh.advertise<serialport::ALL_UAVs_LLA>("/all_uavs_position/global", 10);
    //【发布】系统启动指令
    ros::Publisher takeoff_flag_pub = nh.advertise<std_msgs::Bool>("/px4_command/takeoff_flag", 10);

    // 发布无人机的位置信息的准备
    serialport::ALL_UAVs_LLA uavs_lla_arry;
    geographic_msgs::GeoPoint geo_msg[4];   //由飞机数量固定长度
    std::vector< geographic_msgs::GeoPoint> geo_arry;

    // 串口设置***************************************************************************************
    try 
    { 
        //设置串口属性，并打开串口 
        ser.setPort("/dev/ttyUSB0"); 
        ser.setBaudrate(115200); 
        serial::Timeout to = serial::Timeout::simpleTimeout(ttimeout); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    catch (serial::IOException& e) 

    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    } 
    //检测串口是否已经打开，并给出提示信息 
    if(ser.isOpen()) 
    { 
        ROS_INFO_STREAM("Serial Port initialized"); 
    } 
    else 
    { 
        return -1; 
    }
    // ************************************************************************************************
    // 定义数据统计变量
    struct SINFO
    {
        int send_num;
        int send_r_num;
        float send_r_rate;
        int recv_num;
        int recv_m1_r_num;
        int recv_m2_r_num;
        float recv_r_rate;
        int fail_num;
    };
    SINFO s_info = {};

    // 地面站信息模拟
    // life_buff[0] = 0xa4;
    // life_buff[1] = 0xc0;
    // life_buff[2] = 0x0;
    // life_buff[3] = 0x1;
    // life_buff[4] = data_crc(life_buff,Blen_life-2);   //  校验
    // life_buff[5] = 0x4a;

    //计时相关参数
    bool timeout = 0, loop_state, send_state =1,recv_state = 1;
    clock_t start_time=0,end_time=0;
    int time_dif=0;

    // 正式开始工作
    while(ros::ok()) 
    {
        // 地面站模拟
        // while (ros::ok())
        // {
        //     int gb;
        //     cout << "please input gb:";
        //     cin >> gb;
        //     if (gb)
        //     {
        //         for (int i=0;i<10;i++)
        //         {
        //             len_send = ser.write(life_buff,Blen_life);
        //         }
        //     }
        //     loop_rate.sleep();
        // }
        
        // cout <<  endl;
        //执行回调函数
        ros::spinOnce();
        //  更新自身信息****************************************************
        // uav3.longitude = 133.2135413;
        // uav3.latitude = 33.2165385;
        // uav3.altitude = 3.12156348;

        uav3.longitude    = abs(uav_current_state.longitude);
        uav3.latitude     = abs(uav_current_state.latitude);
        uav3.altitude     = abs(uav_current_state.position[2]);
        //  时间溢出判断****************************************************
        // end_time = clock();
        // time_dif = end_time - start_time;
        // if (time_dif > 160)     //160个机器时间
        //     timeout = 1; 
        // else
        //     timeout = 0; 
        //  发送自身信息****************************************************
        // if (send_state == 1)//|| timeout)
        // {
            //  信息的构造
            send_buff[0] = 0xa4;    //  帧头
            send_buff[1] = 0xc3;    //  命令
            send_buff[2] = 0x01;    //  命令
            send_buff[3] = split(uav3.longitude,1);  send_buff[4] = split(uav3.longitude,2); send_buff[5] = split(uav3.longitude,3); send_buff[6] = split(uav3.longitude,4);
            send_buff[7] = split(uav3.latitude,1);      send_buff[8] = split(uav3.latitude,2);     send_buff[9] = split(uav3.latitude,3);     send_buff[10] = split(uav3.latitude,4);
            send_buff[11] = split(uav3.altitude,1);    send_buff[12] = split(uav3.altitude,2);   send_buff[13] = split(uav3.altitude,3);  send_buff[14] = split(uav3.altitude,4);
            send_buff[15] = data_crc(send_buff,Blen_child-2);   //  校验
            send_buff[16] = 0x4a;   //  帧尾
            //  信息发送
            len_send = ser.write(send_buff,Blen_child);
            // ros::Duration(0.005).sleep(); //sleep for 5 ms
            //  效果记录
            s_info.send_num++;
            // ros::Duration(0.05).sleep(); //sleep for 50 ms
            if (len_send == Blen_child)
            {
                // cout << "send successful!" << endl;
                s_info.send_r_num++;
            }
            //  重置时间信息
            // timeout = 0;
            // start_time = clock();
        // }
        //  接收信息****************************************************
        // recv_state = 1;
        // while(recv_state && !timeout)
        // {
            numtime++;
            if(numtime%4 ==0)
            {
            numtime=0;
            lend_recv = ser.read(recv_buff,readnum); //接收所有飞机的数据
            if (lend_recv) 
            {
                s_info.recv_num++;
                cout << "lend_recv:" << lend_recv <<endl;
                // for (int i =0;i<lend_recv;i++)
                // {
                //     printf("%#x",recv_buff[i]);
                // }
                // cout << endl;
            }
            // 信息的解析
            int i = 0;
            while (i < lend_recv)
            {
                if (recv_buff[i] == 0xa4 && recv_buff[i+Blen_host-1] == 0x4a)
                {
                    // 接收消息1
                    if (recv_buff[i+1] == 0xf1 && recv_buff[i+2] == 0x01 && (lend_recv-i) >= Blen_host)
                    {
                        // cout << "11111111111111" << endl;
                        // 校验位检验
                    if (data_crc(&recv_buff[i],Blen_host-2) == recv_buff[i+Blen_host-2])
                    {
                            recv_state = 1;
                            s_info.recv_m1_r_num++;
                            uav1.longitude = merge(recv_buff[i+3],recv_buff[i+4],recv_buff[i+5],recv_buff[i+6]);
                            uav1.latitude =     merge(recv_buff[i+7],recv_buff[i+8],recv_buff[i+9],recv_buff[i+10]);
                            uav1.altitude =     merge(recv_buff[i+11],recv_buff[i+12],recv_buff[i+13],recv_buff[i+14]);
                            uav2.longitude = merge(recv_buff[i+15],recv_buff[i+16],recv_buff[i+17],recv_buff[i+18]);
                            uav2.latitude =     merge(recv_buff[i+19],recv_buff[i+20],recv_buff[i+21],recv_buff[i+22]);
                            uav2.altitude =     merge(recv_buff[i+23],recv_buff[i+24],recv_buff[i+25],recv_buff[i+26]);
                            // uav3.longitude = merge(recv_buff[i+27],recv_buff[i+28],recv_buff[i+29],recv_buff[i+30]);
                            // uav3.latitude =     merge(recv_buff[i+31],recv_buff[i+32],recv_buff[i+33],recv_buff[i+34]);
                            // uav3.altitude =     merge(recv_buff[i+35],recv_buff[i+36],recv_buff[i+37],recv_buff[i+38]);
                            uav4.longitude = merge(recv_buff[i+39],recv_buff[i+40],recv_buff[i+41],recv_buff[i+42]);
                            uav4.latitude =     merge(recv_buff[i+43],recv_buff[i+44],recv_buff[i+45],recv_buff[i+46]);
                            uav4.altitude =     merge(recv_buff[i+47],recv_buff[i+48],recv_buff[i+49],recv_buff[i+50]);
                            takeoff_flag_recv = recv_buff[i+51];
                            break;
                    }
                        else
                        {
                            recv_state = 0; 
                            s_info.fail_num++; 
                            i = i + Blen_host;
                        }
                    }
                    else
                        i++;
                }
                else
                    i++;
            }
            // 判断系统启动指令并发布出去
            if (takeoff_flag_recv & 0x04)
                takeoff_flag.data = 1;
            else
                takeoff_flag.data = 0;
            takeoff_flag_pub.publish(takeoff_flag);
            
        //显示信息*************************************************************************
        // cout << "uav1.longitude:" << uav1.longitude << "  " << "uav1.latitude:" << uav1.latitude << "  " << "uav1.altitude:" << uav1.altitude << endl;
        // cout << "uav2.longitude:" << uav2.longitude << "  " << "uav2.latitude:" << uav2.latitude << "  " << "uav2.altitude:" << uav2.altitude << endl;
        // cout << "uav3.longitude:" << uav3.longitude << "  " << "uav3.latitude:" << uav3.latitude << "  " << "uav3.altitude:" << uav3.altitude << endl;
        // cout << "uav4.longitude:" << uav4.longitude << "  " << "uav4.latitude:" << uav4.latitude << "  " << "uav4.altitude:" << uav4.altitude << endl;
        //发布话题信息********************************************************************************
        geo_msg[0].longitude = uav1.longitude;
        geo_msg[0].latitude     = uav1.latitude;
        geo_msg[0].altitude     = uav1.altitude;
        geo_msg[1].longitude = uav2.longitude;
        geo_msg[1].latitude     = uav2.latitude;
        geo_msg[1].altitude     = uav2.altitude;
        geo_msg[2].longitude = uav3.longitude;
        geo_msg[2].latitude     = uav3.latitude;
        geo_msg[2].altitude     = uav3.altitude;
        geo_msg[3].longitude = uav4.longitude;
        geo_msg[3].latitude     = uav4.latitude;
        geo_msg[3].altitude     = uav4.altitude;
        geo_arry.push_back(geo_msg[0]);
        geo_arry.push_back(geo_msg[1]);
        geo_arry.push_back(geo_msg[2]);
        geo_arry.push_back(geo_msg[3]);
        uavs_lla_arry.all_uavs_lla = geo_arry;
        uavs_lla_pub.publish(uavs_lla_arry);
        geo_arry.clear();
            }
        //数据处理
        // uav4= {};uav1 = {};uav2 = {};uav3 = {};uav_ID=0;
        //处理ROS的信息，比如订阅消息,并调用回调函数 
        // ros::spinOnce(); 
        loop_state = loop_rate.sleep();
        // cout << "loop_state:  " << loop_state << endl;
       // cout << "crc_fail:" << s_info.fail_num << endl;
        cout << "recv_base_num:" << s_info.recv_m1_r_num << endl;
        cout << "recv_num: " << s_info.recv_num << endl;
        cout << "send_num: " << s_info.send_num << endl;
    } 
    return 0;
}
